#include "ArmorPlate.h"

ArmorPlate::ArmorPlate()
{
    enemy_color = BLUEENEMY;    //敌方目标为蓝色
    // enemy_color = REDENEMY;     //敌方目标为红色(!!!阈值与蓝色有差别，possibly需要重新调参)
}

ArmorPlate::~ArmorPlate()
{
    // 把画的东西清除一下(其实就是清除里边缓存的数)
    Lrect.clear();
    B_points.clear();
    contours.clear();
    armor_point_last.clear();
    Lrect.clear();
}


double ArmorPlate::dis(Point p1, Point p2)  //求两点距离
{
    double dis;

    dis = sqrt(pow(abs(p1.x - p2.x), 2) + pow(abs(p1.y - p2.y), 2));

    return dis;
}

//计算两个点之间的距离的平方，用于比较
double ArmorPlate::distanceSquared(const Point2f& p1, const Point2f& p2)
{
    return (p1.x - p2.x) * (p1.x -p2.x) + (p1.y - p2.y) * (p1.y - p2.y);
}

//计算矩形中心点
Point2f ArmorPlate::calculateCenter(const Point2f points[4])
{
    Point2f center;
    center.x = 0;
    center.y = 0;
    for(int i = 0; i < 4; ++i)
    {
        center.x += points[i].x;
        center.y += points[i].y;
    }

    center.x /= 4;
    center.y /= 4;

    return center;
}

//计算点相对于中心点的角度
double ArmorPlate::calculateAngle(const Point2f& point, const Point2f& center)
{
    return atan2(point.y - center.y, point.x - center.x);
}

// //对点进行逆时针排序比较
// bool ArmorPlate::compareByAngle(const Point2f& p1, const Point2f& p2, const Point2f& center)
// {
//     double angle1 = calculateAngle(p1, center);
//     double angle2 = calculateAngle(p2, center);
//     if(angle1 != angle2)
//         return angle1 < angle2;
//     //如果角度相同，则比较距离中心点的距离
//     return distanceSquared(p1, center) < distanceSquared(p2, center);
// }

//排序矩形的四个角点
vector<Point2f> ArmorPlate::sortPointsClockwise(const Point2f points[4])
{
    vector<Point2f> sortedPoints(points, points + 4);
    Point2f center = calculateCenter(sortedPoints.data());

    // //使用稳定排序，以保持具有相同角度的点的原始顺序，逆时针
    // sort(sortedPoints.begin(), sortedPoints.end(),
    //     [&](const Point2f& p1, const Point2f& p2){
    //         return compareByAngle(p1, p2, center);
    //     });
    //根据角度进行排序，顺时针
    sort(sortedPoints.begin(), sortedPoints.end(),
        [&](const Point2f& p1, const Point2f& p2) {
            double angle1 = calculateAngle(p1, center);
            double angle2 = calculateAngle(p2, center);
            if(angle1 != angle2)
            {
                return angle1 > angle2;     //顺时针排序
            }
            //如果角度相同，则比较距离中心点的距离
            return dis(p1, center) < dis(p2, center);
        });
    return sortedPoints;
}

void ArmorPlate::autoArmorPlate()       //找装甲板
{
    imgPreprocessor(src_image);
}

void ArmorPlate::imgPreprocessor(Mat& image)      //装甲板预处理
{
    resize(image, image, Size(640, 480));
    image.copyTo(img);      //深拷贝（缺陷 较为消耗算力）

    for(int i = 0; i < image.cols - 1; i++) //消除白色影响（非线性滤波）
    {
        for(int j = 0; j < image.rows - 1; j++)
        {
            int b = image.at<Vec3b>(j,i)[0];
            int g = image.at<Vec3b>(j,i)[1];
            int r = image.at<Vec3b>(j,i)[2];

            if(b > 120 && g > 120 && r > 120)
            {
                image.at<Vec3b>(j,i)[0] = 0;
                image.at<Vec3b>(j,i)[1] = 0;
                image.at<Vec3b>(j,i)[2] = 0;
            }
        }
    }

    split(image, channels);     //通道分离
    B_R = channels.at(enemy_color);  //提取颜色通道
    threshold(B_R, th_b, 210, 255, THRESH_BINARY);     //二值化,阈值给定考虑：180，210阈值在后续两种滤波之下会消除最小灯条轮廓
    medianBlur(th_b, th_b, 11);                 //中值滤波
    GaussianBlur(th_b, resultG, Size(3,5), 3);  //高斯低通滤波
    Canny(resultG, resultG, 100, 200, 3);       //Canny算法

    //最小外接矩形 + 矩的计算
    findContours(resultG, contours, 0, 2);  //轮廓点检测
    cout << "轮廓点数：" << contours.size() <<endl;

    for(int i = contours.size() - 1; i >= 0; i--)   //筛选单个符合几何条件矩形点位(对单个矩形限制：长宽比、角度)
    {
        RotatedRect rrect = minAreaRect(contours[i]);
        Point2f points[4];
        rrect.points(points);
        Point2f cpt = rrect.center;

        if(((dis(points[0], points[1]) / dis(points[1], points[2])) > 2.0 ||
            (dis(points[1], points[2]) / dis(points[1], points[0])) > 2.0 )  /*长宽比条件限制*/&&
            (rrect.angle > -25 || rrect.angle < -65)  /*角度限制*/)
        {
            i -= 1;
            continue;
        }

        if(0 <= i)
        {                     /*pop出不符合条件点位*/
            auto it = contours.begin() + i;
            contours.erase(it);
        }
    }

    for(int i = contours.size() - 1; i >= 0; i--)   //筛选灯条相关矩形点位(对多个矩形进行相关比较)
    {
        RotatedRect rrect = minAreaRect(contours[i]);
        Point2f points[4];
        rrect.points(points);
        Point2f cpt = rrect.center;

        bool judge = false;  //条件判断标志位设立

        for(int j = contours.size() -1; j >= 0; j--)    //再次遍历contours，对coutours[i]进行相关性比较
        {
            RotatedRect nrect = minAreaRect(contours[j]);
            Point2f pointn[4];
            nrect.points(pointn);
            Point2f cpn = nrect.center;

            int blue = img.at<Vec3b>(cpn)[0];
            int green = img.at<Vec3b>(cpn)[1];
            int red = img.at<Vec3b>(cpn)[2];

            if(i == j)      //排除自身的相关性
                continue;
            if(1)//条件判断(后边加)
            {
                judge = true;      //条件满足标志位赋值
                Lrect.push_back(rrect);    //中心点位存入
                circle(img, cpn, 2, Scalar(0,0,255), 2, 8, 0);  //中心点标记
                break;
            }
        }

        if(judge == true)     //满足条件判断
            continue;

        if(0 <= i)  //移除不满足条件的矩形
        {
            auto it = contours.begin() + i;
            contours.erase(it);
        }
    }

    cout << "轮廓点数：" << contours.size() <<endl;

    for(int i = 0; i < Lrect.size(); i++)   //装甲板矩形获取
    {
        for(int j = 0; j < Lrect.size(); j++)
        {
            if(i == j)
                continue;
            if(20 > abs(Lrect[i].angle - Lrect[j].angle) && abs(Lrect[i].center.y - Lrect[j].center.y) < 50)    //限制条件判断
            {
                Point2f cen((Lrect[i].center.x + Lrect[j].center.x) / 2, (Lrect[j].center.y + Lrect[i].center.y) / 2);  //取得理论上亮灯条的中心点

                vector<Point2f> tmp_p;
                tmp_p.push_back(cen);
                tmp_p.push_back(Point(Lrect[i].center.x, Lrect[i].center.y + Lrect[i].size.width / 2));     //light 1
                tmp_p.push_back(Point(Lrect[i].center.x, Lrect[i].center.y - Lrect[i].size.width / 2));     //light 1 finished
                tmp_p.push_back(Point(Lrect[j].center.x, Lrect[j].center.y - Lrect[j].size.width / 2));     //light 2
                tmp_p.push_back(Point(Lrect[j].center.x, Lrect[j].center.y + Lrect[j].size.width / 2));     //light 2 finished

                /***在这里进行修改***/
                // light_width = Lrect[i].size.width;
                // light_height = Lrect[i].size.height;

                if(1 > (abs(tmp_p[1].y - tmp_p[2].y) / abs(Lrect[i].size.height)))  //判断灯条的宽高比是否正常
                {
                    light_flag = false; //宽高比不正常
                    B_points.clear();
                    continue;
                }
                else
                {
                    light_flag = true;
                    B_points.push_back(tmp_p);
                }

                P_yaw = (320 - cen.x) * 0.01;
                P_pitch = (240 - cen.y) * 0.01;
            }
        }
    }

    for(int i = 0; i < B_points.size(); i++)    //遍历B_points，装甲板矩形勾画
    {
        Point2f points[4];
        points[0] = B_points[i][1];
        points[1] = B_points[i][2];
        points[2] = B_points[i][3];
        points[3] = B_points[i][4];

        armor_center = B_points[i][0];

        for(int j = 0; j < 4; j++)  //框出最小矩形
        {
            if(3 == j)      //首尾相接
            {
                if(0 == i)
                {
                    armor_point[0] = points[0];
                    armor_point[1] = points[1];
                    armor_point[2] = points[2];
                    armor_point[3] = points[3];

                    armor_point_last = sortPointsClockwise(armor_point);

                    armor_width = dis(points[3], points[0]);
                    armor_height = dis(points[0], points[1]);

                    armor_point[0] = armor_point_last[3];
                    armor_point[1] = armor_point_last[0];
                    armor_point[2] = armor_point_last[1];
                    armor_point[3] = armor_point_last[2];
                }
                // line(img, armor_point[3], armor_point[0], Scalar(255, 0, 255), 2, 8, 0);
                line(img, points[3], points[0], Scalar(255, 0, 255), 2, 8, 0);
                break;
            }
            line(img, points[j], points[j+1], Scalar(255, 0, 255), 2, 8, 0);        //接出三边
            // line(img, armor_point[j], armor_point[j+1], Scalar(255, 0, 255), 2, 8, 0);        //接出三边
        }
        // circle(img, armor_center, 2, Scalar(255, 0, 255), 2, 8, 0);      //中心点标记
        circle(img, armor_center, 2, Scalar(255, 0, 255), 2, 8, 0);      //中心点标记
    }

    /*放在析构函数里边*/
    // // 把画的东西清除一下(其实就是清除里边缓存的数)
    // Lrect.clear();
    // B_points.clear();
    // contours.clear();

}
